#! /usr/bin/python
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('actionlib')
roslib.load_manifest('pr2_controllers_msgs')
roslib.load_manifest('geometry_msgs')
roslib.load_manifest('block_build_msgs')

import rospy
import actionlib
import time
from actionlib_msgs.msg import *
from pr2_controllers_msgs.msg import *
from geometry_msgs.msg import *
from std_msgs.msg import *
from block_build_msgs.msg import *

"""
   Author  : Andy Huang
   Date    : June 2012

   head_manager.py
"""

"""
   Ros node that managers the orientation of the robot head
   Responds to speech localization and speech recognition messages
   Listens to commands sent through the topic HEAD_COMMAND_TOPIC_NAME as std_msgs/String messages

   Implemented actions (for HEAD_COMMAND_TOPIC_NAME):
      lookAt(x,y,z)           Points the robot's head at the point (x,y,z) relative to the /base_link tf frame
      shakeHead(n_times)      Shakes the robot's head back and forth n_times
      lookAtTable()           Points the robot's head back down at the table
"""

HEAD_COMMAND_TOPIC_NAME = "/mit/head_command"                                 # Topic for manually sending lookAt(x,y,z), shakeHead(n), and lookAtTable() commands to head_manager
OBJECT_MANAGER_SWITCH_TOPIC_NAME = "/mit/object_manager_switch"               # Topic to switch on and off updating in ObjectManager depending on whether or head is pointed at the table
SPEECH_LOCALIZATION_TOPIC_NAME = "/speech/localization"                       # Topic that receieves speech localization output relative to the kinect frame
SPEECH_RECOGNITION_TOPIC_NAME = "/speech/recognition_output"            # Topic that constantly receives output from the speech_recognition software
SPEECH_MODE_TOPIC_NAME = "/speech/change_mode"                                # Topic to switch the speech the speech recognition software between command and word learning modes
NL_INPUT_TOPIC_NAME = "/blocknlp/nl_input"                                    # Topic to send a natural language command to pr2_commander
SLU_COMMAND_PARSER_STATUS_TOPIC_NAME = "/blocknlp/slu_command_parser_status"  # Topic to check to see if a NL command sent to block_builder has finished executing
NL_OUTPUT_TOPIC_NAME = "/blocknlp/nl_output"                                  # Topic used to determine when pr2_commander sends a command to block_builder

class HeadManager:

  point_head_client = None
  head_command_listener = None
  object_manager_switch_pub = None
  speech_localization_listener = None
  speech_recognition_listener = None
  speech_recognition_pub = None
  nl_input_pub = None
  inProcessCommand = False
  listeningForCommand = True     # whether or not the robot is ready to process an output from the speech recognition software
  useListenMode = False

  def __init__(self):
    self.point_head_client = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
    self.head_command_listener = rospy.Subscriber(HEAD_COMMAND_TOPIC_NAME, String, self.headCommandListener)
    self.object_manager_switch_pub = rospy.Publisher(OBJECT_MANAGER_SWITCH_TOPIC_NAME, Bool)
    self.speech_localization_listener = rospy.Subscriber(SPEECH_LOCALIZATION_TOPIC_NAME, Point, self.speechLocalizationCallback)
    self.speech_recognition_listener = rospy.Subscriber(SPEECH_RECOGNITION_TOPIC_NAME, String, self.speechRecognitionCallback)
    self.speech_recognition_pub = rospy.Publisher(SPEECH_MODE_TOPIC_NAME, String)
    self.nl_input_pub = rospy.Publisher(NL_INPUT_TOPIC_NAME, String)

  def headCommandListener(self, msg):
    rospy.loginfo("in Head command listener msg : ")
    rospy.loginfo(msg.data)
    self.head_command_parser(msg)

  def head_command_parser(self, msg):
    lookAt = self.lookAt
    shakeHead = self.shakeHead
    lookAtTable = self.lookAtTable

    eval(msg.data)
    return msg

  def lookAt(self, x, y, z):
    self.inProcessCommand = True
    self.object_manager_switch_pub.publish(Bool(False))
    self.lookAtPoint(x, y, z, 'base_link')
    self.inProcessCommand = False

  def shakeHead(self, n_times):
    self.inProcessCommand = True
    self.object_manager_switch_pub.publish(Bool(False))
    for i in range(n_times):
      # Looks at a point forward (x = 5m), slightly left (y = 1m), and 1.2m up
      self.lookAtPoint(5.0, 1.0, 1.2, 'base_link')
      # Looks at a point forward (x = 5m), slightly right (y = -1m), and 1.2m up
      self.lookAtPoint(5.0, -1.0, 1.2, 'base_link')
    self.inProcessCommand = False

  def lookAtTable(self):
    self.inProcessCommand = True
    self.lookAtPoint(0.5, 0.0, -0.4, 'torso_lift_link')
    self.object_manager_switch_pub.publish(Bool(True))
    self.inProcessCommand = False

  def lookAtPoint(self, x, y, z, frame_id):
    # Create the goal object
    g = PointHeadGoal()
    g.target.header.frame_id = frame_id
    g.target.point.x = x
    g.target.point.y = y
    g.target.point.z = z
    g.min_duration = rospy.Duration(1.0)

    # Send the goal
    rospy.loginfo("Sending point head goal")
    self.point_head_client.send_goal(g)
    self.point_head_client.wait_for_result()
    if self.point_head_client.get_state() == GoalStatus.SUCCEEDED:
      rospy.loginfo("Succeeded")
    else:
      rospy.loginfo("Fail")

  def speechLocalizationCallback(self, msg):
    (x,y,z) = (msg.x, msg.y, msg.z)
    print "-----"
    print "Received Speech Localization Output: (%d,%d,%d)" %(x,y,z)
    self.lookAtPoint(x,y,z,'base_link')
    #self.lookAtPoint(x,y,z,'openni_ir_link')    # Relative to the kinect
    time.sleep(3) # Necessary delay to make sure that the robot does not interpret the sound of its head moving as a verbal input
    self.listeningForCommand = True
    if self.useListenMode:
      self.speech_recognition_pub.publish("Start")

  def speechRecognitionCallback(self, msg):
    # Ignore all messages unless the robot is waiting for a verbal command from the user
    print "-----"
    print "Received Speech Recognition Input: ", msg.data

    if self.listeningForCommand or not self.useListenMode:
      self.listeningForCommand = False

      if self.useListenMode:
        self.speech_recognition_pub.publish("Stop")
      self.lookAtTable()
      rospy.loginfo("Sending NL Command...")
      self.nl_input_pub.publish(String(msg.data))

      # Check to see if pr2_commander successfully processes the input NL command
      try: msg = rospy.wait_for_message(NL_OUTPUT_TOPIC_NAME,SLUCommand,5.0)
      except rospy.exceptions.ROSException:
        print "ERROR: Could not process nl_input command in pr2_commander - Timeout Exceeded"
        print "Execution failed"
      else:
        # If pr2_commander is successful, wait for a response from block_builder before proceeding
        print "Evaluating in block_builder: ",msg.command
        try:
          rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,10)
        except rospy.exceptions.ROSException:
          print "ERROR: Command Execution has taken too long - Timeout Exceeded"
          print "Execution failed"
        else:
          rospy.loginfo("Execution Complete")
    else:
      rospy.loginfo("Ignoring speech. listeningForCommand" + self.listeningForCommand + " useListenMode" + self.useListenMode)

  def spin(self):
    rospy.loginfo('Wait for point_head client')
    self.point_head_client.wait_for_server()

    rospy.loginfo('Initialized')
    while not rospy.is_shutdown():
      rospy.sleep(30)

if __name__ == '__main__':
  try:
    rospy.init_node('head_manager')

    hm = HeadManager()
    hm.spin()
  except rospy.ROSInterruptException: pass
